// Functions for controlling Futaba S3003 servo and servo's modified to motor function
//
//	Servo/Motor Connections:
//	White wire:
//	Servo/motor 1: P1.3 - Pin 20
//	Servo/motor 2: P1.5 - Pin 22
//	Servo/motor 3: P3.4 - Pin 5
//	Servo/motor 4: P3.5 - Pin 6
// 	
//	Red wire:  + 5VDC powersupply
//	Black wire: Ground (Powersupply and microcontroller kit)
//
// 	Input values for the set_servo(module, value) function:
//	
// 	module = 1 for servo/motor 1 ect.
// 	Range for value is 0 - 1000
//
// 	SH/2008-10-30


#include <reg66x.h>
#include "dispport.h"
#include <stdio.h>



sbit CEX3 = P3^4;
sbit CEX4 = P3^5;

unsigned int puls_value_to_set[] = {0,0,0,0,0};
unsigned char puls_value_flag[] = {0,0,0,0,0};

void init_PCA()
{
	CCAP0L = 0;
	CCAP0H = 0;
	CCAP2L = 0;
	CCAP2H = 0;
	CCAP3L = 0;
	CCAP3H = 0;
	CCAP4L = 0;
	CCAP4H = 0;
	CMOD = 0x00;
	CCAPM0 = 0x4D;
	CCAPM2 = 0x4D;
	CCAPM3 = 0x4D;
	CCAPM4 = 0x4D;
	EC = 1; 
	EA = 1;
	CR = 1;
}


unsigned char set_servo(unsigned char module, unsigned int value)
{
	if(module > 4) 
	{
		return 0;
	}
	if(module == 0) 
	{
		return 0;
	}
	module--;
	
	if(value > 1000) 
	{
		value = 1000;
	}

	puls_value_flag[module] = 0;
	puls_value_to_set[module] = 1840 + (unsigned int)(value*1.84);
	puls_value_flag[module] = 1;

	return 0;
}

void PCA_match(void) interrupt 6
{
static unsigned int compare_value[] = {0,0,0,0,0};
static unsigned int puls_value[] = { 1840, 1840, 1840, 1840, 1840};

if(CCF0)
	{
	CCF0 = 0;
	if(CEX0 == 1) compare_value[0] += puls_value[0];
	else compare_value[0] += 36800 - puls_value[0];
	CCAP0L = (unsigned char)(compare_value[0] & 0x00ff);
	CCAP0H = (unsigned char)(compare_value[0] >>8); 
	if(puls_value_flag[0]==1) 
		{
		puls_value[0] = puls_value_to_set[0];
		puls_value_flag[0] = 0;
		}
	}
if(CCF2)
	{
	CCF2 = 0;
	if(CEX2 == 1) compare_value[1] += puls_value[1];
	else compare_value[1] += 36800 - puls_value[1];
	CCAP2L = (unsigned char)(compare_value[1] & 0x00ff);
	CCAP2H = (unsigned char)(compare_value[1] >>8); 
	if(puls_value_flag[1]==1) 
		{
		puls_value[1] = puls_value_to_set[1];
		puls_value_flag[1] = 0;
		}
	}

if(CCF3)
	{
	CCF3 = 0;
	if(CEX3 == 1) compare_value[2] += puls_value[2];
	else compare_value[2] += 36800 - puls_value[2];
	CCAP3L = (unsigned char)(compare_value[2] & 0x00ff);
	CCAP3H = (unsigned char)(compare_value[2] >>8); 
	if(puls_value_flag[2]==1) 
		{
		puls_value[2] = puls_value_to_set[2];
		puls_value_flag[2] = 0;
		}
	}

if(CCF4)
	{
	CCF4 = 0;
	if(CEX4 == 1) compare_value[3] += puls_value[3];
	else compare_value[3] += 36800 - puls_value[3];
	CCAP4L = (unsigned char)(compare_value[3] & 0x00ff);
	CCAP4H = (unsigned char)(compare_value[3] >>8); 
	if(puls_value_flag[3]==1) 
		{
		puls_value[3] = puls_value_to_set[3];
		puls_value_flag[3] = 0;
		}
	}

}


